/*
Copyright (c) [2019年5月1日] [吴超]
[MBT_studio] is licensed under Mulan PSL v2.
You can use this software according to the terms and conditions of the Mulan PSL v2.
You may obtain a copy of Mulan PSL v2 at:
		 http://license.coscl.org.cn/MulanPSL2
THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
See the Mulan PSL v2 for more details.
*/
#include "stdafx.h"
#include "ODE刚体.h"


//auto my_ground = chrono::ChBodyEasyBox(60, 2, 60, 1.0, true, true);

S_ODE刚体::S_ODE刚体(S_ODE核心* core, S_物理几何体& geom, S_物理材质& 材质, uint32 key) {
	m_物理引擎枚举 = E_物理引擎::E_物理引擎_ODE;
	m_Type = E_物理体类型::E_物理体_刚体;

	switch (geom.m_Type) {
	case E_物理碰撞几何类型::E_碰撞几何_方体:
		{
			
		}
		break;
	case E_物理碰撞几何类型::E_碰撞几何_凸壳:
		{
			
		}
		//m_刚体 = chrono::ChBodyEasyConvexHull(g->m_Points, g->m_密度);
		break;
	default:
		break;
	}

	//Mat44f mat;
	//f_构建单位矩阵(&mat);
	
	m_Body = dBodyCreate(core->m_物理世界);
	dGeomSetBody((dGeomID)geom.f_get几何体物理对象(), m_Body);

}

S_ODE刚体::~S_ODE刚体() {
	dBodyDestroy(m_Body);
}

void S_ODE刚体::f_setLocRot(const vec3& loc, const vec3& r) {
	dBodySetPosition(m_Body, loc.x, loc.y, loc.z);
	//NewtonBodySetOmega(m_刚体, (const float32*)&loc);
}

void S_ODE刚体::f_setPos(const vec3& l)
{
}

void S_ODE刚体::f_setRot(const vec3& r)
{
	vec4 q = f_graph_欧拉角转四元数(r);
	float64 _q[4] = { q.x, q.y, q.z, q.w };
	dBodySetQuaternion(m_Body, _q);
}

void S_ODE刚体::f_setVel(const vec3& l)
{
}

vec3 S_ODE刚体::f_getLocation()
{
	return vec3();
}










S_ODE静态::S_ODE静态(S_ODE核心* core, S_物理几何体& geom, S_物理材质& 材质, uint32 key) {
	m_物理引擎枚举 = E_物理引擎::E_物理引擎_ODE;
	m_Type = E_物理体类型::E_物理体_静态;

	switch (geom.m_Type) {
	case E_物理碰撞几何类型::E_碰撞几何_方体:
	{

	}
	break;
	case E_物理碰撞几何类型::E_碰撞几何_凸壳:
	{

	}
	//m_刚体 = chrono::ChBodyEasyConvexHull(g->m_Points, g->m_密度);
	break;
	default:
		break;
	}

	m_Body = dBodyCreate(core->m_物理世界);
	auto g = (dGeomID)geom.f_get几何体物理对象();
	dGeomSetBody(g, m_Body);
	//dBodySetMass(m_Body, 1.0);
	//dGeomSetOffsetPosition()
	//dBodyDisable(m_Body);
	dBodySetGravityMode(m_Body, 0);
	//dBodySetKinematic(m_Body);
	//dMass mass;
	//dMassSetZero(&mass);
	//dMassSetBox(&mass, 1, 0.2, -5, 1);
	//dBodySetMass(m_Body, &mass);

}

S_ODE静态::~S_ODE静态() {
	dBodyDestroy(m_Body);
}


void S_ODE静态::f_setLocRot(const vec3& loc, const vec3& r) {
	//Mat44f mat;
	//f_构建移动矩阵(&mat, loc.x, loc.y, loc.z);
	dBodySetPosition(m_Body, loc.x, loc.y, loc.z);
}

void S_ODE静态::f_setPos(const vec3& l) {

}

void S_ODE静态::f_setRot(const vec3& r) {
	vec4 q = f_graph_欧拉角转四元数(r);
	float64 _q[4] = { q.x, q.y, q.z, q.w };
	dBodySetQuaternion(m_Body, _q);
}

void S_ODE静态::f_setVel(const vec3& l) {

}

vec3 S_ODE静态::f_getLocation() {
	return vec3();
}



